#include "SteeringVelocityMatch.h"

#include <cassert>

#include "ai/steering/SteeringOutput.h"
#include "math/Utils.h"
#include "physic/PhysicConstants.h"

VelocityMatchData::VelocityMatchData(const Vector3* srcLinearVel /*= nullptr*/,
                                     const Vector3* targetLinearVel /*= nullptr*/,
                                     const float timeToTarget /*= 0.0f*/)
                                     : mSrcLinearVel(srcLinearVel)
                                     , mTargetLinearVel(targetLinearVel)
                                     , mTimeToTarget(timeToTarget)
{
    assert(mTimeToTarget > 0.0f || areEquals(mTimeToTarget, 0.0f));
}

void VelocityMatchData::set(const Vector3& srcLinearVel,
         const Vector3& targetLinearVel,
         const float timeToTarget)
{
    assert(timeToTarget > 0.0f || areEquals(timeToTarget, 0.0f));

    mSrcLinearVel = &srcLinearVel;
    mTargetLinearVel = &targetLinearVel;  
    mTimeToTarget = timeToTarget;
}

void velocityMatch(const VelocityMatchData * const data, 
                   SteeringOutput * const outputs,
                   const uint32_t numData)
{
    assert(data);
    assert(outputs);
    assert(numData > 0);

    // PARALLEL_FOR
    for (size_t i = 0; i < numData; ++i) {
        const Vector3& srcLinearVel = *data[i].mSrcLinearVel;
        const Vector3& targetLinearVel = *data[i].mTargetLinearVel;
        const float timeToTarget = data[i].mTimeToTarget;
        SteeringOutput& output = outputs[i];

        assert(timeToTarget > 0.0f || areEquals(timeToTarget, 0.0f));

        // Acceleration tries to get to the target linear velocity
        output.mLinearVel = targetLinearVel;
        output.mLinearVel -= srcLinearVel;
        output.mLinearVel *= 1.0f / timeToTarget;

        // Clamp linear velocity to max acceleration if necessary
        {
            const float sqrLen = output.mLinearVel.sqrLength();
            if (MAX_SPEED * MAX_SPEED < sqrLen) {
                output.mLinearVel.setLength(MAX_SPEED);
            }
        }
    }
}